'''
Created on Sep 17, 2010

@author: diego


'''
import cv
import matplotlib
import matplotlib.pyplot as plt


import numpy
from SURFUtils import findPairsNaive, drawKeypoints,compare_desc, drawPoints,\
    getHomography, findPairsFlann, buildSearchKDTree


global figureNumber

def plotPoints(pointSet,color):
    global figureNumber
    color=str(color)+'o'
    plt.figure(figureNumber)
    for (x,y) in pointSet:
        plt.plot(x,y,color)

    
    figureNumber=figureNumber+1
            

def drawReferenceRectangle(bookTransform):
    cv.Line(bookTransform, (100, 100), (150, 100), cv.RGB(100, 0, 0))
    cv.Line(bookTransform, (100, 100), (100, 200), cv.RGB(0, 0, 100))
    cv.Line(bookTransform, (150, 100), (150, 200), cv.RGB(0, 100, 0))
    cv.Line(bookTransform, (100, 200), (150, 200), cv.RGB(100, 100, 100))
    cv.Line(bookTransform, (100, 100), (150, 200), cv.RGB(100, 100, 100))



if __name__ == '__main__':
    global figureNumber
    figureNumber=1
    modelMatchedKeypoints=[]
    dstMatchedKeypoints=[]
    storage=cv.CreateMemStorage(0)
    params=(0,3000,3,1)
    modelImageGR=cv.LoadImage("../resources/krishna2.jpg",cv.CV_LOAD_IMAGE_GRAYSCALE)
    modelImageRGB=cv.LoadImage("../resources/krishna2.jpg")
    capture=cv.CaptureFromCAM(1)
    
    modelSURF=cv.ExtractSURF(modelImageGR, None, storage, params)
    (modelKeyp,modelDesc)=modelSURF
    flannKDTree=buildSearchKDTree(modelSURF)
    
    drawKeypoints(modelImageGR,modelKeyp)
    
    warpedImage=cv.CreateImage((640,480), 8, 1)
 
    bookTransform=cv.CreateMat(480,640,cv.CV_32FC3)
    drawReferenceRectangle(bookTransform)
    bookTransformed=cv.CloneMat(bookTransform)
        
    cv.ShowImage("referenceSrc", bookTransform)
    cv.ShowImage("model", modelImageRGB)
    
    frameGR=cv.CreateImage((640, 480), 8, 1)
    
    while(True):
        frame=cv.QueryFrame(capture)
        cv.CvtColor(frame, frameGR, cv.CV_RGB2GRAY)
        
        dstSURF=cv.ExtractSURF(frameGR, None, storage, params)
        (dstKeyp,dstDesc)=dstSURF
    
#        matchedPairs=findPairsNaive(modelSURF,dstSURF)
        matchedPairs=findPairsFlann(modelSURF,dstSURF,flannKDTree)
        
        modelMatchedKeypoints=map((lambda x : x[0]),matchedPairs)
        dstMatchedKeypoints=map((lambda x : x[1]),matchedPairs)
    
        drawPoints(frame, dstMatchedKeypoints)
    
        if len(dstMatchedKeypoints)>=4:
            mapMatrix=getHomography(modelMatchedKeypoints, dstMatchedKeypoints)
        else:
            print "Not enough POIs"
            
        
        
        cv.WarpPerspective(bookTransform, bookTransformed,mapMatrix) 
        cv.WarpPerspective(modelImageGR, warpedImage,mapMatrix)
    
        cv.ShowImage("CurrentFrame", frame)
        cv.ShowImage("referenceDst", bookTransformed)
        cv.ShowImage("warpedImage", warpedImage)
        cv.WaitKey(20)
    
    
    
    

    
    
    
    
    
    
#    compare_desc(modelDesc, dstDesc)
#        for ((x, y), laplacian, size, dir, hessian) in keypoints:
#            print "x=%d y=%d laplacian=%d size=%d dir=%f hessian=%f" % (x, y, laplacian, size, dir, hessian)
#    cv.Circle(frame, (x,y), size, (200,50,50))
    

